/********************************************************************************/
/*																				*/
/*	Project:	Tianwan NPP FHM CCS												*/
/*	Library:	Twn_Mov_xy.dll													*/
/*	Functions:	Contains functions to calculate tarjectories across the plant	*/
/*				considering TV rotation and fastest path.						*/
/*	Author:		Matteo Tavella													*/
/*	Date:		05/03/2002														*/
/*																				*/
/********************************************************************************/
#define ALONE_TNW_MOV_XY
#include "Twn_Mov_xy.h"


BOOL WINAPI DllMain(HINSTANCE hinstDLL,  // handle to DLL module
					DWORD fdwReason,     // reason for calling function
					LPVOID lpReserved )  // reserved
					{
	LONG	res;
	LPDWORD	type;
	LPBYTE	data;
	LPDWORD size;
	HKEY	hKey;
	TCHAR	szProductType[MY_BUFSIZE];
    DWORD	dwBufLen = MY_BUFSIZE;
	DWORD	active;
    
	size = 0;
	data = 0;
	type = 0;
	active = 0;
	
	switch(fdwReason){ 
		case DLL_PROCESS_ATTACH:
			RegOpenKeyEx(	HKEY_LOCAL_MACHINE, 
							TEXT("SOFTWARE\\ABB\\Power Generation Portal\\Tianwan\\Twn_Mov_xy"),
							0,
							KEY_QUERY_VALUE,
							&hKey);
			res = RegQueryValueEx(	hKey,
									TEXT("LogPath"),
									NULL,
									NULL,
									(LPBYTE)szProductType,
									&dwBufLen);
			_stprintf_s(MXY_LogPath, (TCHAR*)szProductType);

			dwBufLen = MY_BUFSIZE;
			res = RegQueryValueEx(	hKey,
									TEXT("LogActive"),
									NULL,
									NULL,
									(LPBYTE)&active,
									&dwBufLen);
			if((DWORD)active == 0)
				MXY_LogActive = false;
			else
				MXY_LogActive = true;
			break;
		default:
			break;
	}
    return TRUE;  // Successful DLL_PROCESS_ATTACH.
}

/*------------------------------------------------------------------------------
** Function:		CheckIntermediateRot
**
** Description:		Check if the rotation (also partial) from the angle ang_S 
**					to the angle ang_D is possible in the cell (VirtX, VirtY).
**
** Input:			VirtX		- X virtual coordinate of the cell to check
**					VirtY		- Y virtual coordinate of the cell to check
**					ang_S		- Start rotation angle.
**					ang_D		- Stop rotation angle				
** Output:			rot			- Rotation is possible on 1 or 2 range.
**
** Return:			RETSTAT		- RT_NORMAL	If an angle has been retrieved.
**								- RT_FAILED If there's no angle in the range
** Inserita da F.Bonelli 20/07/06
*/

RETSTAT CheckIntermediateRot(int VirtX, int VirtY, float ang_S, float ang_D, int *rot){
	FILEIDX		cellFile;
	RETCOND		ret;
	RETSTAT		retSt;
	GENIDX		zoneIdx;
	int			i, cellNum;
	t_cell		*cell;
	
	*rot = 0;
	cell = new t_cell;
	GetZoneIdxVirtCoo(VirtX, VirtY, &zoneIdx);
	cellFile.num = F_CE01 + zoneIdx - 1;
	retSt = GetZoneCellNum(zoneIdx, &cellNum);
	if (retSt != RT_NORMAL)
		return(RT_FAILED);
	for(i = 1; i<=cellNum; i++){
		ret = FileRead(&cellFile, i, 1, cell, sizeof(t_cell));
		if (ret != R_NORMAL)
			return(RT_FAILED);
		if((cell->Virt_X == VirtX) && (cell->Virt_Y == VirtY)){
			if((cell->Rot_Flag == ROT_ALLOWED) && 
			   ((ang_S >= cell->Rot_Range.range_1[0]) &&
			    (ang_S <= cell->Rot_Range.range_1[1]) &&
//				(ang_D >= cell->Rot_Range.range_1[0]) &&
//			    (ang_D <= cell->Rot_Range.range_1[1]) &&
				(cell->Rot_Range.range_1[0] != -1) && (cell->Rot_Range.range_1[1] != -1))){
				if(ang_D > ang_S)
					if(ang_S < cell->Rot_Range.range_1[1])
						*rot = 1;
				if(ang_D < ang_S)
					if(ang_S > cell->Rot_Range.range_1[0])
						*rot = 1;
				cell = NULL;
				delete cell;
				return (RT_NORMAL);
			}
			if((cell->Rot_Flag == ROT_ALLOWED) && 
			   ((ang_S >= cell->Rot_Range.range_2[0]) &&
			    (ang_S <= cell->Rot_Range.range_2[1]) &&
//				(ang_D >= cell->Rot_Range.range_2[0]) &&
//			    (ang_D <= cell->Rot_Range.range_2[1]) &&
				(cell->Rot_Range.range_2[0] != -1) && (cell->Rot_Range.range_2[1] != -1))){
				if(ang_D > ang_S)
					if(ang_S < cell->Rot_Range.range_2[1])
						*rot = 2;
				if(ang_D < ang_S)
					if(ang_S > cell->Rot_Range.range_2[0])
						*rot = 2;
				cell = NULL;
				delete cell;
				return (RT_NORMAL);
			}
		}
	}
	delete cell;
	return(RT_NORMAL);
}

/********************************************************************************/
/*																				*/
/*	Function:		Mov_xy_Exec													*/
/*	Description:	Take a movement between 2 phys. points and calls Exec_seq_xy*/
/*					for each micro movement (MOVX and MOVY) necessary for the	*/
/*					move. If the movement is 2 steps, the routine chose teh way	*/
/*					checking if the intermediate point is in a forbidden area.	*/
/*					The first try is on X movement.								*/
/*	Input:			ZoneIdx		-	Zone where the movement is executed			*/
/*					X_Act		-	Source phisycal X coordinate.				*/
/*					X_Nex		-	Destination phisycal X coordinate.			*/
/*					Y_Act		-	Source phisycal Y coordinate.				*/
/*					Y_Nex		-	Destination phisycal Y coordinate.			*/
/*	Return:			PH_NORMAL	-	Success.									*/
/*					PH_FAILED	-	Failure.									*/
/*	ErrCode:		1000 - 1100													*/
/*																				*/
/********************************************************************************/
PHASTAT Mov_xy_Exec (GENIDX ZoneIdx, float X_Act, float X_Nex, float Y_Act, float Y_Nex){
	RETSTAT		stat;
	PHASTAT		phRet;
	RETCOND		retCnd;
	float		piu_X;
	float		piu_Y;
	float		meno_X;
	float		meno_Y;
	int			muovi_X;
	int			muovi_Y;
	float		dynX, dynY;
	float		falv_x;
	float		falv_y;
	int			mastType;


	stat = GetDynCurrentMast(&mastType);
	if(stat != RT_NORMAL){
		_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: cannot retrieve dynamic mast used."));
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(1));
		return (PH_FAILED);
	}

	if((mastType == FALC_MAST) || (mastType == DMP_MAST))
		GetFALVOffset(&falv_x, &falv_y);
	else{
		falv_x = 0.0;
		falv_y = 0.0;
	}

	stat = GetDynMastPos(mastType, &dynX, &dynY); //24092005 add

	//piu_X  = (float)(X_Act - falv_x + TOLL_MM); //24092005 del
	//piu_Y  = (float)(Y_Act - falv_y + TOLL_MM); //24092005 del
	//meno_X = (float)(X_Act - falv_x - TOLL_MM); //24092005 del
	//meno_Y = (float)(Y_Act - falv_y - TOLL_MM); //24092005 del

	piu_X  = (float)(dynX - falv_x + TOLL_MM);
	piu_Y  = (float)(dynY - falv_y + TOLL_MM);
	meno_X = (float)(dynX - falv_x - TOLL_MM);
	meno_Y = (float)(dynY - falv_y - TOLL_MM);


	if ( isInRange(X_Nex - falv_x,meno_X,piu_X) ){
		muovi_X = 0;
	}
	else{
		muovi_X = 1;
	}
	if ( isInRange(Y_Nex - falv_y,meno_Y,piu_Y) ){
		muovi_Y = 0;
	}
	else{
		muovi_Y = 1;
	}
	
	if((muovi_X == 0) && (muovi_Y == 0))
		return(PH_NORMAL);
	
	//stat = GetDynMastPos(mastType, &dynX, &dynY);	//24092005 del
	if(stat != RT_NORMAL){
		_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: cannot retrieve dynamic MM position."));
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(1));
		return (PH_FAILED);
	}
	
	//if(((dynX - falv_x) < meno_X) || ((dynX - falv_x) > piu_X)){  //24092005 del
	//if(((X_Act - falv_x) < meno_X) || ((X_Act - falv_x) > piu_X)){  //24092005 add
	if ( !isInRange(X_Act - falv_x,meno_X,piu_X) ){
		_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: move start X coordinates mismatch with dynamic data."));
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(2));
		return (PH_COOMISM);
	}

	//if(((dynY - falv_y) < meno_Y) || ((dynY - falv_y) > piu_Y)){  //24092005 del
	//if(((Y_Act - falv_y) < meno_Y) || ((Y_Act - falv_y) > piu_Y)){  //24092005 add
	if ( !isInRange(Y_Act - falv_y,meno_Y,piu_Y) ){
		_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: move start Y coordinates mismatch with dynamic data."));
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(3));
		return (PH_COOMISM);
	}
	
	/* Se devo muovere solo sulla X ... */
	if ((muovi_X == 1) && (muovi_Y == 0)){
		muovi_X = 0;
		if(mastType == MAIN_MAST){
			phRet = Exec_Seq_xy(MICRO_MOVEX, X_Nex, 0);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Bridge movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(4));
				return(phRet);
			}
			stat = SetDynMmxPos(X_Nex);
		}
		else if((mastType == FALC_MAST) || (mastType == DMP_MAST)){
			phRet = Exec_Seq_xy(MICRO_MOVEX, X_Nex - falv_x, 0);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Bridge movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(4));
				return(phRet);
			}
			SetDynFCxyPos(X_Nex, dynY);
		}
			
		retCnd = PathStepAdd(mastType, X_Nex, Y_Act, PATH_TYPE_PLAN);
		if(retCnd != R_NORMAL){
			_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: unalbe to add step in Path file."));
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(5));
			return (PH_FAILED);	
		}
	}

	/* Se devo muovere solo sulla Y ... */

	if((muovi_X == 0) && (muovi_Y == 1)){
		muovi_Y = 0;
		if(mastType == MAIN_MAST){
			phRet = Exec_Seq_xy(MICRO_MOVEY, 0, Y_Nex);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Trolley movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(6));
				return (phRet);
			}
			stat = SetDynMmyPos(Y_Nex);
		}
		else if((mastType == FALC_MAST) || (mastType == DMP_MAST)){
			phRet = Exec_Seq_xy(MICRO_MOVEY, 0, Y_Nex - falv_y);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Trolley movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(6));
				return (phRet);
			}
			SetDynFCxyPos(X_Nex, Y_Nex);
		}
			
		retCnd = PathStepAdd(mastType, X_Act, Y_Nex, PATH_TYPE_PLAN);
		if(retCnd != R_NORMAL){
			_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: unalbe to add step in Path file."));
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(7));
			return (PH_ERRADDPATH);	
		}
	}

	/* Se devo muovere sulla X e sulla Y utilizzo il movimento elementare composito.*/
	if ((muovi_X == 1) && (muovi_Y == 1)){
		if(mastType == MAIN_MAST){
			phRet = Exec_Seq_xy(MICRO_MOVEXY, X_Nex, Y_Nex);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Bridge,Trolley movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(8));
				return (phRet);
			}
			stat = SetDynMmxPos(X_Nex);
			stat = SetDynMmyPos(Y_Nex);
		}
		else if((mastType == FALC_MAST) || (mastType == DMP_MAST)){
			phRet = Exec_Seq_xy(MICRO_MOVEXY, X_Nex - falv_x, Y_Nex - falv_y);
			if(phRet != PH_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error planning Bridge,Trolley movement."));
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(8));
				return (phRet);
			}
			SetDynFCxyPos(X_Nex, Y_Nex);
		}
			

		retCnd = PathStepAdd(mastType, X_Nex, Y_Nex, PATH_TYPE_PLAN);
		if(retCnd != R_NORMAL){
			_stprintf_s(MXY_LogMsg, _T("Mov_xy_Exec error: unalbe to add step in Path file."));
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(9));
			return (PH_ERRADDPATH);	
		}
	}
	
	return (PH_NORMAL);
}

/********************************************************************************/
/*																				*/
/*	Function:		Mov_xy_local												*/
/*	Description:	Build a path with TV rotation information within a zone		*/
/*	Input:			S_Virt_X	-	Source virtual X coordinate.				*/
/*					S_Virt_Y	-	Source virtual Y coordinate.				*/
/*					D_Virt_X	-	Destination virtual X coordinate.			*/
/*					D_Virt_Y	-	Destination virtual Y coordinate.			*/
/*					S_Phis_X	-	Source phisycal X coordinate.				*/
/*					S_Phis_Y	-	Source phisycal Y coordinate.				*/
/*					D_Phis_X	-	Destination phisycal X coordinate.			*/
/*					D_Phis_Y	-	Destination phisycal Y coordinate.			*/
/*					TV_Angle_S	-	TV angle start position.					*/
/*					TV_Angle_D	-	TV angle stop position. -1 for any angle in	*/
/*									dest. range.								*/
/*	Return:			PH_NORMAL	-	Success										*/
/*	ErrCodes:		1101 - 1200
/*																				*/
/*	Author:			Matteo Tavella												*/
/*	Date:			05/03/2002													*/
/********************************************************************************/
PHASTAT Mov_xy_local(int S_Virt_X, int S_Virt_Y, int D_Virt_X, int D_Virt_Y,
					 float S_Phis_X, float S_Phis_Y, float D_Phis_X, float D_Phis_Y,
					 float TV_Angle_S, float TV_Angle_D){

	RETSTAT		retSt;
	RETCOND		retCnd;
	PHASTAT		phastat;
	GENIDX		MainZoneIdx;
	GENIDX		ReactZoneIdx;
	GENIDX		IOZoneIdx;
	GENIDX		FADSZoneIdx;
	GENIDX		HecaZoneIdx_1;
	GENIDX		HecaZoneIdx_2;
	GENIDX		startZone;
	GENIDX		stopZone;
	GENIDX		tempZone;
	GENIDX		realSourceIdx;
	GENIDX		realDestIdx;
//	GENIDX		zoneRotIdx;
	ZONENAME	startZoneName;
	ZONENAME	stopZoneName;
	FILEIDX		cellFile;
	TAGIDX		tagIdx;
	TAGVALEX	tagVal;

	t_cell		cell;
	TV_angle	rangeS, rangeD, rangeC;
	phas_struc	phas;
	
	float		DiffAngle;
	float		sx, sy, dx, dy, ix, iy;
	double		dist, dist_si, dist_id;
	int			svx, svy, dvx, dvy, srf, drf, crf, all_rot_s, all_rot_d, all_rot_i, all_rot_c, ivx, ivy, i, cellNum, i_rot;
	BOOL		rotS, rotD, useVirtS, useVirtD, rotExe, foundInt, rotNeeded, tvMounted;
	float		cx, cy;	
	int			cvx, cvy;
	int			markFlag;
	int			currPhas;
	int			mast;


	svx = 0;
	svy = 0;
	dvx = 0;
	dvy = 0;
	ivx = 0;
	ivy = 0;
	rotS = false;
	rotD = false;
	rotExe = false;
	rotNeeded = false;
	tvMounted = false;
	foundInt = false;
	all_rot_s = 0;
	all_rot_d = 0;
	all_rot_i = 0;
	cx = 0.0;
	cy = 0.0;

	//Preparo gli Idx delle zone. //
	retSt = GetZoneIdx(ZONE_MAIN, &MainZoneIdx);
	retSt = GetZoneIdx(ZONE_REACTOR, &ReactZoneIdx);
	retSt = GetZoneIdx(ZONE_IO, &IOZoneIdx);
	retSt = GetZoneIdx(ZONE_HECA_1, &HecaZoneIdx_1);
	retSt = GetZoneIdx(ZONE_HECA_2, &HecaZoneIdx_2);
	retSt = GetZoneIdx(ZONE_FFDS, &FADSZoneIdx);

	GetDynCurrentMast(&mast);

	//Controllo che la TV sia montata//
	retCnd = GetTagIdx(TWN_TV_MOUNT, &tagIdx);
	if(retCnd != R_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving TV_MOUNT tag Idx."), MXY_ERR_CODE(101));
		return(PH_INVSOUR);
	}

	retCnd = TagRead(&tagIdx, &tagVal);
	if(retCnd != R_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving TV_MOUNT tag value."), MXY_ERR_CODE(102));
		return(PH_INVSOUR);
	}

	if(GtBit((short int)(tagVal.value.val_di) , 15) == 1)
		tvMounted = true;

	if(TV_Angle_S < 0.0)
		TV_Angle_S = 0.0;
	else if(TV_Angle_S > 315.0)
		TV_Angle_S = 315.0;

	//Controllo se ho le coordinate fisiche o virtuali per source. In ogni caso prelevo
	//coordinate fisiche e virtuali di partenza e arrivo.
	if((S_Virt_X == 0) && (S_Virt_Y == 0))
		useVirtS = false;
	else if((S_Virt_X == 0) && (S_Virt_Y != 0)){
		MXY_LogFileWrite(_T("ERR"), _T("Invalid source coordinates."), MXY_ERR_CODE(103));
		return(PH_INVSOUR);
	}
	else
		useVirtS = true;

	if(useVirtS){
		svx = S_Virt_X;
		svy = S_Virt_Y;
		retSt = GetCellVirtCoo(svx, svy, &cell);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source cell info."), MXY_ERR_CODE(104));
			return(PH_INVSOUR);
		}
		sx = cell.Phys_X;
		sy = cell.Phys_Y;
		rangeS = cell.Rot_Range;
		srf = cell.Rot_Flag;
	}
	else{
		retSt = GetZoneNearCell(S_Phis_X, S_Phis_Y, &svx, &svy, &srf, &rangeS, false);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source near cell."), MXY_ERR_CODE(105));
			return(PH_INVSOUR);
		}
		sx = S_Phis_X;
		sy = S_Phis_Y;
	}
	
	if((D_Virt_X == 0) && (D_Virt_Y == 0))
		useVirtD = false;
	else if((D_Virt_X == 0) && (D_Virt_Y != 0)){
		return(PH_INVSOUR);
		MXY_LogFileWrite(_T("ERR"), _T("Invalid destination coordinates."), MXY_ERR_CODE(106));
	}
	else
		useVirtD = true;

	if(useVirtD){
		dvx = D_Virt_X;
		dvy = D_Virt_Y;
		retSt = GetCellVirtCoo(dvx, dvy, &cell);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. cell info."), MXY_ERR_CODE(107));
			return(PH_INVSOUR);
		}
		dx = cell.Phys_X;
		dy = cell.Phys_Y;
		rangeD = cell.Rot_Range;
		drf = cell.Rot_Flag;
	}
	else{
		retSt = GetZoneNearCell(D_Phis_X, D_Phis_Y, &dvx, &dvy, &drf, &rangeD, false);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source near cell."), MXY_ERR_CODE(108));
			return(PH_INVSOUR);
		}
		dx = D_Phis_X;
		dy = D_Phis_Y;
	}
	
	//Prelevo gli Idx delle zone di partenza e di arrivo, se sono diverse esco, se sono
	//sottozone di Z2 proseguo, e le riconduco a Z2 (ZONE_MAIN).//
	retSt = GetZoneIdxVirtCoo(svx, svy, &startZone);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone Idx."), MXY_ERR_CODE(109));
		return (PH_INVSOUR);
	}

	retSt = GetZoneName(startZone, startZoneName);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone name."), MXY_ERR_CODE(110));
		return (PH_INVSOUR);
	}

	retSt = GetZoneIdxVirtCoo(dvx, dvy, &stopZone);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. zone Idx."), MXY_ERR_CODE(111));
		return (PH_INVDEST);
	}

	retSt = GetZoneName(stopZone, stopZoneName);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. zone name."), MXY_ERR_CODE(112));
		return (PH_INVDEST);
	}

	retSt = GetSuperZoneFromName(startZoneName, &tempZone);
	if(retSt == RT_FAILED){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source super zone Idx."), MXY_ERR_CODE(113));
		return (PH_FAILED);
	}
	else{
		realSourceIdx = startZone;
		if(tempZone == MainZoneIdx)
			startZone = tempZone;	
	}

	retSt = GetSuperZoneFromName(stopZoneName, &tempZone);
	if(retSt == RT_FAILED){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. super zone Idx."), MXY_ERR_CODE(114));
		return (PH_FAILED);
	}
	else{
		realDestIdx = stopZone;
		if(tempZone == MainZoneIdx)
			stopZone = tempZone;	
	}

	//Eseguo i controlli sulle zone di partenza e arrivo. Controllo se i movimenti sono di carico
	//o di scarico per utilizzare il mast giusto.//
	if(startZone != stopZone){
		MXY_LogFileWrite(_T("ERR"), _T("Error source zone differs from dest. zone."), MXY_ERR_CODE(115));
		return (PH_FAILED);
	}

	//Controllo se devo eseguire un passo intermedio, per evitare l'angolo...//
	//phastat = CheckMoveFeasibility(sx, sy, dx, dy, &cx, &cy);
	phastat = CheckMoveFeasibilityEx(sx, sy, dx, dy, TV_Angle_S, &cx, &cy);
	if(phastat != PH_NORMAL)
		return(phastat);
	
	if(tvMounted && (mast == MAIN_MAST)){
		//Controllo se gli angoli di partenza e arrivo sono legali per le celle.//
		retSt = CheckAngleInRange(rangeS, TV_Angle_S);
		if(retSt == RT_FAILED){
			MXY_LogFileWrite(_T("ERR"), _T("Error checking source TV angle."), MXY_ERR_CODE(116));
			return(PH_INVSOUR);
		}
		//Se l'angolo di destinazione � definito controllo se appartiene all'intervallo della
		//cella di destinazione, altrimenti imposto come angolo di destinazione un angolo legale
		//per la cella di destinazione.
		if(TV_Angle_D != -1){
			retSt = CheckAngleInRange(rangeD, TV_Angle_D);
			if(retSt == RT_FAILED){
				MXY_LogFileWrite(_T("ERR"), _T("Error checking source TV angle."), MXY_ERR_CODE(117));
				return(PH_INVSOUR);
			}
		}
		else{
			markFlag = 0;

			retSt = GetDynCurrentPhase(&currPhas);
			if(retSt == RT_FAILED){
				MXY_LogFileWrite(_T("ERR"), _T("Error cretrieving current phase."), MXY_ERR_CODE(117));
				return(PH_INVSOUR);
			}

			retSt = GetPhase(currPhas, &phas);
			if(retSt == RT_FAILED){
				MXY_LogFileWrite(_T("ERR"), _T("Error cretrieving current phase data."), MXY_ERR_CODE(117));
				return(PH_INVSOUR);
			}

			if(!useVirtD)
				markFlag = 0;
			else if(((phas.SV_X == dvx) &&  (phas.SV_Y == dvy))			||
				    ((phas.DV_X == dvx) &&  (phas.DV_Y == dvy))){
				if(phas.markReq != 0)
					markFlag = 1;
				else
					markFlag = 0;
			}
			else
				markFlag = 0;

			retSt = GetBestAngle(rangeD, TV_Angle_S, markFlag, &TV_Angle_D);
			if(retSt == RT_FAILED){
				MXY_LogFileWrite(_T("ERR"), _T("Error retrieving legal dest. angle."), MXY_ERR_CODE(118));
				return(PH_INVSOUR);
			}
			else{
				_stprintf_s(MXY_LogMsg, _T("Destination angle set to %3.0f�."), TV_Angle_D);
				MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
			}
		}

		//Calcolo la differenza tra gli angoli di arrivo e partenza.//
		DiffAngle = TV_Angle_S - TV_Angle_D;
		if(DiffAngle < 0.0)
			DiffAngle = (float)(DiffAngle * -1.0);
		if(DiffAngle >= TOLL_MM_GR)
			rotNeeded = true;
		else
			MXY_LogFileWrite(_T("INF"), _T("Rotation is not necessary for movement."), 0);
	}
	
	//Controllo se posso ruotare la TV in partenza.//
	if(rotNeeded && tvMounted && (mast == MAIN_MAST)){
		retSt = CheckRot(svx, svy, TV_Angle_S, TV_Angle_D, &all_rot_s);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error checking dest. TV angle."), MXY_ERR_CODE(119));
			return(PH_FAILED);
		}
	
		//Controllo se posso ruotare la TV all'arrivo.//
		retSt = CheckRot(dvx, dvy, TV_Angle_S, TV_Angle_D, &all_rot_d);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error checking dest. TV angle."), MXY_ERR_CODE(120));
			return(PH_FAILED);
		}
	}
	
	//Se devo spezzare la traiettoria controllo se posso ruotare nel punto inermedio.//
	if(rotNeeded && tvMounted && !all_rot_s && !all_rot_d && (mast == MAIN_MAST)){
		if((cx != 0.0) && (cy != 0.0)){
			retSt = GetZoneNearCell(cx, cy, &cvx, &cvy, &crf, &rangeC, false);
			if(retSt != RT_NORMAL){
				MXY_LogFileWrite(_T("ERR"), _T("Error retrieving central point near cell."), MXY_ERR_CODE(108));
				return(PH_INVSOUR);
			}
			//Controllo se posso ruotare la TV nel punto centrale.//
			retSt = CheckRot(cvx, cvy, TV_Angle_S, TV_Angle_D, &all_rot_c);
			if(retSt != RT_NORMAL){
				MXY_LogFileWrite(_T("ERR"), _T("Error checking cental TV angle."), MXY_ERR_CODE(120));
				return(PH_FAILED);
			}
			if(all_rot_c == 0)
				all_rot_i = 1;
			else
				all_rot_i = 0;
		}else{
			all_rot_i = 1;
			all_rot_c = 0;
		}
	}
	

	//Se la rotazione � necessaria ma non � possibile modifico la traiettoria.//
	if((mast == MAIN_MAST) && rotNeeded && tvMounted && ((all_rot_s == 0) && (all_rot_d == 0) && (all_rot_c == 0))){
		//Cerco un punto intermedio alla partenza e all'arrivo che mi permetta di 
		//eseguire la rotazione prima della cella finale. Il punto � a distanza minima
		//dalla retta che congiunge i due punti.//

		ivx = 0;
		ivy = 0;
		ix = (float)0.0;
		iy = (float)0.0;
		dist = 999999.9;
		foundInt = false;
		i_rot = 0;

		cellFile.num = F_CE01 + realSourceIdx - 1;
		retSt = GetZoneCellNum(realSourceIdx, &cellNum);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone Idx."), MXY_ERR_CODE(121));
			return(PH_FAILED);
		}
		for(i = 1; i <= cellNum; i++){
			retCnd = FileRead(&cellFile, i, 1, &cell, sizeof(t_cell));
			if(retCnd != R_NORMAL){
				_stprintf_s(MXY_LogMsg, _T("Error reading cell DAFS file n.%d."), cellFile.num);
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(122));
				return(PH_FAILED);
			}
			if(cell.Rot_Flag == 0)
				continue;
			if((cell.Virt_X == 0) && (cell.Virt_Y == 0))
				break;
			retSt = CheckRot(cell.Virt_X, cell.Virt_Y, TV_Angle_S, TV_Angle_D, &i_rot);
			if(retSt != RT_NORMAL){
				MXY_LogFileWrite(_T("ERR"), _T("Error checking intermediate TV angle."), MXY_ERR_CODE(123));
				return(PH_FAILED);
			}
			if(i_rot > 0){
				//Calcolo le distanze fra i punti s,d,i.//
				dist_si = sqrt(((sx - cell.Phys_X) * (sx - cell.Phys_X)) + ((sy - cell.Phys_Y) * (sy - cell.Phys_Y)));
				dist_id = sqrt(((cell.Phys_X - dx) * (cell.Phys_X - dx)) + ((cell.Phys_Y - dy) * (cell.Phys_Y - dy)));
				if((dist_si + dist_id) < dist){
					ix = cell.Phys_X;
					iy = cell.Phys_Y;
					ivx = cell.Virt_X;
					ivy = cell.Virt_Y;
					dist = (dist_si + dist_id);
					foundInt = true;
				}
			}
		}
		//Se non ho trovato il punto nella zona di partenza lo cerco a destinazione..//
		if(realSourceIdx != realDestIdx){
			//ivx = 0;
			//ivy = 0;
			//ix = (float)0.0;
			//iy = (float)0.0;
			//dist = 999999.9;
			//foundInt = false;
			i_rot = 0;

			cellFile.num = F_CE01 + realDestIdx - 1;
			retSt = GetZoneCellNum(realDestIdx, &cellNum);
			if(retSt != RT_NORMAL){
				MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone Idx."), MXY_ERR_CODE(121));
				return(PH_FAILED);
			}
			for(i = 1; i <= cellNum; i++){
				retCnd = FileRead(&cellFile, i, 1, &cell, sizeof(t_cell));
				if(retCnd != R_NORMAL){
					_stprintf_s(MXY_LogMsg, _T("Error reading cell DAFS file n.%d."), cellFile.num);
					MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(122));
					return(PH_FAILED);
				}
				if(cell.Rot_Flag == 0)
					continue;
				if((cell.Virt_X == 0) && (cell.Virt_Y == 0))
					break;
				retSt = CheckRot(cell.Virt_X, cell.Virt_Y, TV_Angle_S, TV_Angle_D, &i_rot);
				if(retSt != RT_NORMAL){
					MXY_LogFileWrite(_T("ERR"), _T("Error checking intermediate TV angle."), MXY_ERR_CODE(123));
					return(PH_FAILED);
				}
				if(i_rot > 0){
					//Calcolo le distanze fra i punti s,d,i.//
					dist_si = sqrt(((sx - cell.Phys_X) * (sx - cell.Phys_X)) + ((sy - cell.Phys_Y) * (sy - cell.Phys_Y)));
					dist_id = sqrt(((cell.Phys_X - dx) * (cell.Phys_X - dx)) + ((cell.Phys_Y - dy) * (cell.Phys_Y - dy)));
					if((dist_si + dist_id) < dist){
						ix = cell.Phys_X;
						iy = cell.Phys_Y;
						ivx = cell.Virt_X;
						ivy = cell.Virt_Y;
						dist = (dist_si + dist_id);
						foundInt = true;
					}
				}
			}
		}
		if(foundInt){

			cx = 0;
			cy = 0;
			if(!useVirtS)
			{
				svx=0;
				svy=0;
			}
			phastat = Mov_xy_local(svx, svy, ivx, ivy, sx, sy, ix, iy, TV_Angle_S, TV_Angle_D);
			if ( phastat != PH_NORMAL ){
				_stprintf_s(MXY_LogMsg, _T("Error executing move from (%d,%d) to (%d,%d)."), sx, sy, ix, iy);
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(124));
				return (phastat);
			}
			else{
				sx = ix;
				sy = iy;
				svx = ivx;
				svy = ivy;
			}
		}
		else{
			_stprintf_s(MXY_LogMsg, _T("Cannot find intermediate points for rotation in zone %s."), startZoneName);	
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, 126);
			return (PH_FAILED);
		}
	}

	//Se posso ruotare all'inizio ruoto.//
	if((all_rot_s != 0) && (all_rot_d == 0) && rotNeeded && tvMounted && (mast == MAIN_MAST)){
		phastat = Exec_Seq_TV(TV_Angle_D);
		if ( phastat != PH_NORMAL ){
			_stprintf_s(MXY_LogMsg, _T("Error executing initial TV rotation to %3.0f�."), TV_Angle_D);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(127));
			return (phastat);
		}
		else{
			_stprintf_s(MXY_LogMsg, _T("Rotation executed to angle %3.0f� at start position."), TV_Angle_D);	
			MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
			rotExe = true;
		}
	}

	//Se devo vado al punto intermedio//
	if((cx != 0.0) && (cy != 0.0) && (mast == MAIN_MAST))
	{
		phastat = Mov_xy_Exec(startZone, sx, cx, sy, cy);
		if ( phastat != PH_NORMAL )
		{
			_stprintf_s(MXY_LogMsg, _T("Error executing move from (%.1f,%.1f) to (%.1f,%.1f)."), sx, sy, cx, cy);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(128));
			return (phastat);
		}
		else
		{
			_stprintf_s(MXY_LogMsg, _T("Executed move to intermediate point (%.1f,%.1f)."), cx, cy);	
			MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
		}

		//se necessario ruoto la TV//
		if((all_rot_c != 0) && (all_rot_d == 0) && rotNeeded && tvMounted && !rotExe)
		{
			phastat = Exec_Seq_TV(TV_Angle_D);
			if ( phastat != PH_NORMAL )
			{
				_stprintf_s(MXY_LogMsg, _T("Error executing intermediate TV rotation to %3.0f�."), TV_Angle_D);
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(127));
				return (phastat);
			}
			else
			{
				_stprintf_s(MXY_LogMsg, _T("Rotation executed to angle %3.0f� at intermediate position."), TV_Angle_D);	
				MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
				rotExe = true;
			}
		}
	}

	//Muovo alla posizione finale.//
	if((cx == 0.0) && (cy == 0.0))
	{
		phastat = Mov_xy_Exec(startZone, sx, dx, sy, dy);
		if ( phastat != PH_NORMAL )
		{
			_stprintf_s(MXY_LogMsg, _T("Error executing move from (%d,%d) to (%d,%d)."), sx, sy, dx, dy);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(128));
			return (phastat);
		}
		else
		{
			_stprintf_s(MXY_LogMsg, _T("Executed move to final cell (%d,%d)."), dvx, dvy);	
			MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
		}
	
		//Se non ho gi� eseguito la rotazione controllo se posso eseguirla alla fine.//
		if((all_rot_d != 0) && rotNeeded && tvMounted && (rotExe == false) && (mast == MAIN_MAST))
		{
			phastat = Exec_Seq_TV(TV_Angle_D);
			if ( phastat != PH_NORMAL )
			{
				_stprintf_s(MXY_LogMsg, _T("Error executing final TV rotation to %3.0f�."), TV_Angle_D);
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(129));
				return (phastat);
			}
			else
			{
				_stprintf_s(MXY_LogMsg, _T("Rotation executed to angle %3.0f� at end position."), TV_Angle_D);	
				MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
				rotExe = true;
			}
		}	
	}
	else
	{
		phastat = Mov_xy_Exec(startZone, cx, dx, cy, dy);
		if ( phastat != PH_NORMAL )
		{
			_stprintf_s(MXY_LogMsg, _T("Error executing move from (%.1f,%.1f) to (%.1f,%.1f)."), cx, cy, dx, dy);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(128));
			return (phastat);
		}
		else
		{
			_stprintf_s(MXY_LogMsg, _T("Executed move to final cell (%d,%d)."), dvx, dvy);	
			MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
		}
	
		//Se non ho gi� eseguito la rotazione controllo se posso eseguirla alla fine.//
		if((all_rot_d != 0) && rotNeeded && tvMounted && (rotExe == false))
		{
			phastat = Exec_Seq_TV(TV_Angle_D);
			if ( phastat != PH_NORMAL )
			{
				_stprintf_s(MXY_LogMsg, _T("Error executing final TV rotation to %3.0f�."), TV_Angle_D);
				MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(129));
				return (phastat);
			}
			else
			{
				_stprintf_s(MXY_LogMsg, _T("Rotation executed to angle %3.0f� at end position."), TV_Angle_D);	
				MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
				rotExe = true;
			}
		}	
	}
	return(PH_NORMAL);
}

/*********************************************************************************************
**********************************************************************************************
**
** Function:		MXY_LogFileWrite
**
** Description:		Print a message in the application log file,
**
** Input:			MXY_cmd			- _T("ERR") - error, _T("WRN") - warning, _T("OK ") - success.
**					Comment			- Message description.
**
** Return:			int				- 1 for success.
**									- -1 otherwise.
** 
**********************************************************************************************
*********************************************************************************************/
int MXY_LogFileWrite(TCHAR MXY_cmd[4], TCHAR *Comment, int errNum){
	FILE		*LogFile;
	TCHAR		newLine = '\n';
	SYSTEMTIME	timeStamp;
	TCHAR		timeStr[100];
	
	if(MXY_LogActive){

		errno_t err;
		err = _tfopen_s(&LogFile,MXY_LogPath, _T("a"));
		if (err != 0)
			return -1;

		//LogFile = _tfopen(MXY_LogPath, _T("a"));
		//if(LogFile == NULL)
		//	return -1;
		
		GetSystemTime(&timeStamp);
		_stprintf_s(timeStr, _T("[hh%2d  mm%2d  ss%2d,%3d (GMT)] [%2d-%2d-%4d]"), timeStamp.wHour, timeStamp.wMinute, timeStamp.wSecond, timeStamp.wMilliseconds, timeStamp.wDay, timeStamp.wMonth, timeStamp.wYear);
		if(_tcscmp(MXY_cmd, _T("ERR")) == 0)
			_ftprintf(LogFile,_T("%s   %s  %s Err.N� %d.%c"), timeStr, MXY_cmd, Comment, errNum,  newLine);
		else
			_ftprintf(LogFile,_T("%s   %s  %s%c"), timeStr, MXY_cmd, Comment, newLine);
		if(fclose(LogFile)!=0)
			return -1;
		}
	return 0;
}

/****************************************************************************************/
/*																						*/
/*	Function:		Mov_xy																*/
/*	Description:	Build a path with TV rotation information in different zones		*/
/*	Input:			S_Virt_X	-	Source virtual X coordinate.						*/
/*					S_Virt_Y	-	Source virtual Y coordinate.						*/
/*					D_Virt_X	-	Destination virtual X coordinate.					*/
/*					D_Virt_Y	-	Destination virtual Y coordinate.					*/
/*					S_Phis_X	-	Source phisycal X coordinate.						*/
/*					S_Phis_Y	-	Source phisycal Y coordinate.						*/
/*					D_Phis_X	-	Destination phisycal X coordinate.					*/
/*					D_Phis_Y	-	Destination phisycal Y coordinate.					*/
/*					TV_Angle_D	-	TV angle stop position. -1 for any angle in			*/
/*									dest. range.										*/
/*	Return:			PH_NORMAL	-	Success												*/
/*	ErrCodes:		1201 - 1300															*/
/*																						*/
/*	Author:			Matteo Tavella														*/
/*	Date:			05/03/2002															*/
/*																						*/
/****************************************************************************************/
PHASTAT Mov_xy(int S_Virt_X, int S_Virt_Y, int D_Virt_X, int D_Virt_Y,
			   float S_Phis_X, float S_Phis_Y, float D_Phis_X, float D_Phis_Y, 
			   float TV_Angle_D){
	RETSTAT		retSt;
	RETCOND		retCnd;
	PHASTAT		phastat;
	TAGIDX		tagIdx;
	TAGVALEX	tagVal;
	GENIDX		MainZoneIdx;
	GENIDX		ReactZoneIdx;
	GENIDX		IOZoneIdx;
	GENIDX		FADSZoneIdx;
	GENIDX		HecaZoneIdx_1;
	GENIDX		HecaZoneIdx_2;
	GENIDX		startZone;
	GENIDX		stopZone;
	GENIDX		tempZone;
	GENIDX		realSourceIdx;
	GENIDX		realDestIdx;
	GENIDX		stepStartZoneIdx;
	GENIDX		stepStopZoneIdx;
	GENIDX		zoneDummy;
	t_cell		cellDummy;
	ZONENAME	startZoneName;
	ZONENAME	stopZoneName;
	FILEIDX		zoneFile;

	t_cell		cell;
	t_zone		tzSource;
	LEGALPATH	lPath;
	CONFZONE	cZone;

	TV_angle	rangeS, rangeD;
	float		sx, sy, dx, dy, temp_angle_s, temp_angle_d;
	int			svx, svy, dvx, dvy, ivx, ivy, cevx, cevy, srf, drf, all_rot_s, all_rot_d, i, cor_virt_x, cor_virt_y; 
	int			cor_step, mastType;
	BOOL		rotS, rotD, useVirtS, useVirtD, rotExe, rotNeeded, corrPresent, tvMounted;
	float		TV_Angle_S;
	int			step;
	int			startStep;
	float		eAngle;
	
	svx = 0;
	svy = 0;
	dvx = 0;
	dvy = 0;
	ivx = 0;
	ivy = 0;
	cevx = 0;
	cevy = 0;
	rotS = false;
	rotD = false;
	rotExe = false;
	rotNeeded = true;
	corrPresent = false;
	all_rot_s = 0;
	all_rot_d = 0;
	cZone.zone = 0;
	cZone.n_point = 0;
	mastType = MAIN_MAST;
	zoneFile.num = F_ZONE;

	//Preparo gli Idx delle zone. //
	retSt = GetZoneIdx(ZONE_MAIN, &MainZoneIdx);
	retSt = GetZoneIdx(ZONE_REACTOR, &ReactZoneIdx);
	retSt = GetZoneIdx(ZONE_IO, &IOZoneIdx);
	retSt = GetZoneIdx(ZONE_HECA_1, &HecaZoneIdx_1);
	retSt = GetZoneIdx(ZONE_HECA_2, &HecaZoneIdx_2);
	retSt = GetZoneIdx(ZONE_FFDS, &FADSZoneIdx);

	//Controllo che la TV sia montata//
	retCnd = GetTagIdx(TWN_TV_MOUNT, &tagIdx);
	if(retCnd != R_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving TV_MOUNT tag Idx."), MXY_ERR_CODE(101));
		return(PH_INVSOUR);
	}

	retCnd = TagRead(&tagIdx, &tagVal);
	if(retCnd != R_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving TV_MOUNT tag value."), MXY_ERR_CODE(102));
		return(PH_INVSOUR);
	}

	if(GtBit((short int)(tagVal.value.val_di) , 15) == 1)
		tvMounted = true;
	else
		tvMounted = false;

	retSt = GetDynTVAngle(&TV_Angle_S);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Unable to retrieve dest angle from dyn. properties."), MXY_ERR_CODE(201));
		return(PH_FAILED);
	}

	//Controllo se ho le coordinate fisiche o virtuali per source. In ogni caso prelevo
	//coordinate fisiche e virtuali di partenza e arrivo.
	if((S_Virt_X == 0) && (S_Virt_Y == 0))
		useVirtS = false;
	else if(((S_Virt_X != 0) && (S_Virt_Y == 0)) || ((S_Virt_X == 0) && (S_Virt_Y != 0))){
		MXY_LogFileWrite(_T("ERR"), _T("Invalid source coordinates."), MXY_ERR_CODE(202));
		return(PH_INVSOUR);
	}
	else
		useVirtS = true;

	if((D_Virt_X == 0) && (D_Virt_Y == 0))
		useVirtD = false;
	else if(((D_Virt_X != 0) && (D_Virt_Y == 0)) || ((D_Virt_X == 0) && (D_Virt_Y != 0))){
		return(PH_INVSOUR);
		MXY_LogFileWrite(_T("ERR"), _T("Invalid destination coordinates."), MXY_ERR_CODE(203));
	}
	else
		useVirtD = true;

	
	if(useVirtS){
		svx = S_Virt_X;
		svy = S_Virt_Y;
		retSt = GetCellVirtCoo(svx, svy, &cell);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source cell info."), MXY_ERR_CODE(204));
			return(PH_INVSOUR);
		}
		sx = cell.Phys_X;
		sy = cell.Phys_Y;
		rangeS = cell.Rot_Range;
		srf = cell.Rot_Flag;
	}
	else{
		retSt = GetZoneNearCell(S_Phis_X, S_Phis_Y, &svx, &svy, &srf, &rangeS, false);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source near cell."), MXY_ERR_CODE(205));
			return(PH_INVSOUR);
		}
		sx = S_Phis_X;
		sy = S_Phis_Y;
	}
	
	if(useVirtD){
		dvx = D_Virt_X;
		dvy = D_Virt_Y;
		retSt = GetCellVirtCoo(dvx, dvy, &cell);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. cell info."), MXY_ERR_CODE(206));
			return(PH_INVDEST);
		}
		dx = cell.Phys_X;
		dy = cell.Phys_Y;
		rangeD = cell.Rot_Range;
		drf = cell.Rot_Flag;
	}
	else{
		retSt = GetZoneNearCell(D_Phis_X, D_Phis_Y, &dvx, &dvy, &drf, &rangeD, false);
		if(retSt != RT_NORMAL){
			MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. near cell."), MXY_ERR_CODE(207));
			return(PH_INVDEST);
		}
		dx = D_Phis_X;
		dy = D_Phis_Y;
	}

	//Prelevo gli Idx delle zone di partenza e di arrivo, se sono diverse vedo quali corridoi devo
	//devo attraversare ed seguo il movimento a tratti separati.//
	retSt = GetZoneIdxVirtCoo(svx, svy, &startZone);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone Idx."), MXY_ERR_CODE(208));
		return (PH_INVSOUR);
	}

	retSt = GetZoneName(startZone, startZoneName);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source zone name."), MXY_ERR_CODE(209));
		return (PH_INVSOUR);
	}

	retSt = GetZoneIdxVirtCoo(dvx, dvy, &stopZone);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. zone Idx."), MXY_ERR_CODE(210));
		return (PH_INVDEST);
	}

	retSt = GetZoneName(stopZone, stopZoneName);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. zone name."), MXY_ERR_CODE(211));
		return (PH_INVDEST);
	}

	retSt = GetSuperZoneFromName(startZoneName, &tempZone);
	if(retSt == RT_FAILED){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving source super zone Idx."), MXY_ERR_CODE(212));
		return (PH_FAILED);
	}
	else{
		realSourceIdx = startZone;
		if(tempZone == MainZoneIdx)
			startZone = tempZone;	
	}

	retSt = GetSuperZoneFromName(stopZoneName, &tempZone);
	if(retSt == RT_FAILED){
		MXY_LogFileWrite(_T("ERR"), _T("Error retrieving dest. super zone Idx."), MXY_ERR_CODE(213));
		return (PH_FAILED);
	}
	else{
		realDestIdx = stopZone;
		if(tempZone == MainZoneIdx)
			stopZone = tempZone;	
	}

	//Eseguo i controlli sulle zone di partenza e arrivo. Se muovo da 
	if(startZone != stopZone){
		MXY_LogFileWrite(_T("INF"), _T("Source zone differs from dest. zone: splitting movement."), 0);
		corrPresent = true;
	}
	else{
		if(useVirtS && useVirtD)
			phastat = Mov_xy_local(svx, svy, dvx, dvy, 0.0, 0.0, 0.0, 0.0, TV_Angle_S, TV_Angle_D);
		else if(useVirtS && !useVirtD)
			phastat = Mov_xy_local(svx, svy, 0, 0, 0.0, 0.0, dx, dy, TV_Angle_S, TV_Angle_D);
		else if(!useVirtS && useVirtD)
			phastat = Mov_xy_local(0, 0, dvx, dvy, sx, sy, 0.0, 0.0, TV_Angle_S, TV_Angle_D);
		else
			phastat = Mov_xy_local(0, 0, 0, 0, sx, sy, dx, dy, TV_Angle_S, TV_Angle_D);
		return(phastat);
	}

	//Scrivo nel log l'inizo della mossa.//
	_stprintf_s(MXY_LogMsg, _T("Initializing movement: cell(%d,%d) ---> cell(%d,%d)."), svx, svy, D_Virt_X, D_Virt_Y);
	MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
	
	retSt = GetZoneFinalLegalPath(startZone, stopZone, &lPath);
	if(retSt == RT_NORMAL){
		if(lPath.n_zone <= 0){
			_stprintf_s(MXY_LogMsg, _T("Error: zero lenght legal path."));
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(214));
			return (PH_FAILED);
		}
	}
	else{
		_stprintf_s(MXY_LogMsg, _T("Error retrieving legal path."));
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(215));
		return (PH_FAILED);
	}

	//Se il legal path � di un passo mi preparo ad attraversare un corridoio tra due zone adiacenti.//
	//Cerco il corridoio nelle zone confinanti con quella di partenza.//
	for(step = 0; step < lPath.n_zone; step++){
		if((lPath.n_zone == 1) || (step == 0))
			stepStartZoneIdx = startZone;
		else
			stepStartZoneIdx =  lPath.zone[step - 1];
		stepStopZoneIdx = lPath.zone[step];
		
		//Controllo se le zone di destinazione sono legali per quelle di partenza, utilizzo i
		//legal path definiti nella struttura t_zone.//
		retCnd = FileRead(&zoneFile, stepStartZoneIdx, 1, &tzSource, sizeof(t_zone));
		if(retCnd != R_NORMAL){
			_stprintf_s(MXY_LogMsg, _T("Error reading DAFS file F.ZONE at record n�%d."), startZone);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(216));
			return (PH_FAILED);
		}

		eAngle = 0;

		for(i = 0; i < tzSource.n_conf_zone; i++){
			if(tzSource.conf_zone[i].zone == stepStopZoneIdx){
				cZone = tzSource.conf_zone[i];
				
				retSt = GetDynTVAngle(&temp_angle_s);
				if(retSt != RT_NORMAL){
					MXY_LogFileWrite(_T("ERR"), _T("Unable to retrieve dest angle from dyn. properties."), MXY_ERR_CODE(31));
					return(PH_FAILED);
				}

				if(tzSource.conf_zone[i].n_point == 2){
					retSt = GetZonePhisVirtCoordEx(cZone.corr_point[0][0], cZone.corr_point[0][1], true, &cor_virt_x, &cor_virt_y);
					if(retSt != RT_NORMAL){
						_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve corridor exit point virtual coord."));
						MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
						return (PH_FAILED);
					}
					startStep = 0;
				}
				else{
					if(!tvMounted){
						retSt = GetZonePhisVirtCoordEx(cZone.corr_point[1][0], cZone.corr_point[1][1], true, &cor_virt_x, &cor_virt_y);
						if(retSt != RT_NORMAL){
							_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve corridor exit point virtual coord."));
							MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
							return (PH_FAILED);
						}
						startStep = 1;
					}
					else{
						if(((temp_angle_s >= (float)(tzSource.conf_zone[i].in_angle - TOLL_MM)) && (temp_angle_s <= (float)(tzSource.conf_zone[i].in_angle + TOLL_MM)))){
							retSt = GetZonePhisVirtCoordEx(cZone.corr_point[1][0], cZone.corr_point[1][1], true, &cor_virt_x, &cor_virt_y);
							if(retSt != RT_NORMAL){
								_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve corridor exit point virtual coord."));
								MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
								return (PH_FAILED);
							}
							startStep = 1;
						}
						else{
							retSt = GetZonePhisVirtCoordEx(cZone.corr_point[0][0], cZone.corr_point[0][1], true, &cor_virt_x, &cor_virt_y);
							if(retSt != RT_NORMAL){
								_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve corridor exit point virtual coord."));
								MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
								return (PH_FAILED);
							}
							startStep = 0;
						}
					}
				}

				//Controllo l'angolo da mantenere dipendamente dalla cella in cui comincia il corridoio.//
				retSt = GetZoneVirtPar(cor_virt_x, cor_virt_y, &zoneDummy, &cellDummy);
				if(retSt != RT_NORMAL){
					_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve virtual parameters for corridor entry point."));
					MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(219));
					return (PH_FAILED);
				}
				temp_angle_d = tzSource.conf_zone[i].in_angle;
				//Mi sposto alla posizione di ingresso del corridoio.//
				if(step == 0){
					if(useVirtS){
						phastat = Mov_xy_local(svx, svy, cor_virt_x, cor_virt_y, 0, 0, 0, 0, temp_angle_s, temp_angle_d);
						if(phastat != PH_NORMAL){
							_stprintf_s(MXY_LogMsg, _T("Error: movement to start corridor point returned phastat = %d."), phastat);
							MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(220));
							return (phastat);
						}
						else{
							_stprintf_s(MXY_LogMsg, _T("Executed movement to corridor %s start point (%5.2f,%5.2f)."), cZone.corr_name, cZone.corr_point[0][0], cZone.corr_point[0][1]);
							MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
						}
					}
					else{
						phastat = Mov_xy_local(0, 0, cor_virt_x, cor_virt_y, sx, sy, 0, 0, temp_angle_s, temp_angle_d);
						if(phastat != PH_NORMAL){
							_stprintf_s(MXY_LogMsg, _T("Error: movement to start corridor point returned phastat = %d."), phastat);
							MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(220));
							return (phastat);
						}
						else{
							_stprintf_s(MXY_LogMsg, _T("Executed movement to corridor %s start point (%5.2f,%5.2f)."), cZone.corr_name, cZone.corr_point[0][0], cZone.corr_point[0][1]);
							MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
						}
					}
					
				}
				else{
					phastat = Mov_xy_local(cevx, cevy, cor_virt_x, cor_virt_y, 0, 0, 0, 0, temp_angle_s, temp_angle_d);
					if(phastat != PH_NORMAL){
						_stprintf_s(MXY_LogMsg, _T("Error: movement to start corridor point returned phastat = %d."), phastat);
						MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(220));
						return (phastat);
					}
					else{
						_stprintf_s(MXY_LogMsg, _T("Executed movement to corridor %s start point (%5.2f,%5.2f)."), cZone.corr_name, cZone.corr_point[0][0], cZone.corr_point[0][1]);
						MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
					}
				}

				//Per ogni tratto del corridoio eseguo un movimento elementare. Non muovo la TV.//
				for(cor_step = startStep; cor_step < cZone.n_point - 1; cor_step++){
					phastat = Mov_xy_Exec (cZone.zone, cZone.corr_point[cor_step][0], cZone.corr_point[cor_step + 1][0], 
										   cZone.corr_point[cor_step][1], 
									       cZone.corr_point[cor_step + 1][1]);			
					if(phastat != PH_NORMAL){
						_stprintf_s(MXY_LogMsg, _T("Error: movement to intermediate corridor point returned phastat = %d."), phastat);
						MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(221));
						return (phastat);
					}
					else{
						if(cor_step == cZone.n_point - 2){
							_stprintf_s(MXY_LogMsg, _T("Executed movement to corridor %s final point (%5.2f,%5.2f)."), cZone.corr_name, cZone.corr_point[cor_step + 1][0], cZone.corr_point[cor_step + 1][1]);
							MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
						}
						else{
							_stprintf_s(MXY_LogMsg, _T("Executed movement to corridor %s intermediate point (%5.2f,%5.2f)."), cZone.corr_name, cZone.corr_point[cor_step + 1][0], cZone.corr_point[cor_step + 1][1]);
							MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
						}
					}
						
				}
				
				//Prelevo le coordinate virtuali del punto di uscita del corridoio.//
				retSt = GetZonePhisVirtCoordEx(cZone.corr_point[cZone.n_point - 1][0], cZone.corr_point[cZone.n_point - 1][1], true, &cor_virt_x, &cor_virt_y);
				/*
				if((cor_virt_x % 100)!= 0){
					_stprintf_s(MXY_LogMsg, _T("Error: invalid corridor end point x virtual coord."));
					MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(222));
					return (PH_FAILED);
				}
				
				if((cor_virt_y % 100) != stepStartZoneIdx){
					_stprintf_s(MXY_LogMsg, _T("Error: invalid corridor end point y virtual coord."));
					MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
					return (PH_FAILED);
				}
				*/
				if(retSt != RT_NORMAL){
					_stprintf_s(MXY_LogMsg, _T("Error: cannot retrieve corridor exit point virtual coord."));
					MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
					return (PH_FAILED);
				}
				cevx = cor_virt_x;
				cevy = cor_virt_y;
				eAngle = tzSource.conf_zone[i].out_angle;
				}//End if zona intermedia.//
		}//End for zone confinanti.//
	}//End sugli step del path.//

	//Ruoto la TV....//
	retSt = GetDynTVAngle(&temp_angle_s);
	if(retSt != RT_NORMAL){
		MXY_LogFileWrite(_T("ERR"), _T("Unable to retrieve dest angle from dyn. properties."), MXY_ERR_CODE(31));
		return(PH_FAILED);
	}

	if((temp_angle_s != eAngle) && tvMounted){
		phastat = Exec_Seq_TV(eAngle);
		if(phastat = PH_NORMAL){
			_stprintf_s(MXY_LogMsg, _T("Error: cannot rotate TV mast to corridor exit angle %f."), tzSource.conf_zone[i].out_angle);
			MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(223));
			return (PH_FAILED);
		}
	}

	//Ora sono nel punto di uscita del corridoio attraversato. Muovo alla destinazione//
	if(useVirtD){
		//phastat = Mov_xy_local(cevx, cevy, dvx, dvy, 0, 0, 0, 0, temp_angle_d, TV_Angle_D);
		phastat = Mov_xy_local(cevx, cevy, dvx, dvy, 0, 0, 0, 0, eAngle, TV_Angle_D);
	}
	else{
		//phastat = Mov_xy_local(cevx, cevy, 0, 0, 0, 0, dx, dy, temp_angle_d, TV_Angle_D);
		phastat = Mov_xy_local(cevx, cevy, 0, 0, 0, 0, dx, dy, eAngle, TV_Angle_D);
	}
	if(phastat != PH_NORMAL){
		_stprintf_s(MXY_LogMsg, _T("Error: movement to end corridor point returned phastat = %d."), phastat);
		MXY_LogFileWrite(_T("ERR"), MXY_LogMsg, MXY_ERR_CODE(224));
		return (phastat);
	}
	else{
		_stprintf_s(MXY_LogMsg, _T("Executed movement from corridor %s end point to destination point (%d,%d)."), cZone.corr_name, D_Virt_X, D_Virt_Y);
		MXY_LogFileWrite(_T("INF"), MXY_LogMsg, 0);
	}

	return(PH_NORMAL);
}

/*********************************************************************************************
**********************************************************************************************
**
** Function:		MXY_ActivateLog
**
** Description:		Activate the Log file.
**
** Return:			int				- 0 for success.
**									- -1 otherwise.
** 
**********************************************************************************************
*********************************************************************************************/
int MXY_ActivateLog(void){
	MXY_LogActive = true;
	return 0;
}

/*********************************************************************************************
**********************************************************************************************
**
** Function:		MXY_DeactivateLog
**
** Description:		Deactivate the Log file.
**
** Return:			int				- 0 for success.
**									- -1 otherwise.
** 
**********************************************************************************************
*********************************************************************************************/
int MXY_DeactivateLog(void){
	MXY_LogActive = false;
	return 0;
}

/*********************************************************************************************
**********************************************************************************************
**
** Function:		MXY_SetLogPath
**
** Description:		Cahge the Log file path.
**
** Input:			path			- New Log file path.
** Return:			int				- 0 for success.
**									- -1 otherwise.
** 
**********************************************************************************************
*********************************************************************************************/
int MXY_SetLogPath(TCHAR *path){
	_tcscpy_s(MXY_LogPath, path);
	return 0;
}

/****************************************************************************************/
/*																						*/
/*	Function:		CheckMoveFeasibility												*/
/*	Description:	Check wether a movement is fesible in one step						*/
/*	Input:			sx			-	Movement source physical x							*/
/*					sy			-	Movement source physical y							*/
/*					dx			-	Movement dest. physical x							*/
/*					dy			-	Movement dest. physical y							*/
/*	Output			cx			-	Constraint physical x								*/
/*					cy			-	Constraint physical y								*/
/*																						*/
/*	Return:			PH_NORMAL	-	Success												*/
/*	ErrCodes:		1301 - 1400															*/
/*																						*/
/*	Author:			Matteo Tavella														*/
/*	Date:			13/03/2002															*/
/*																						*/
/****************************************************************************************/
PHASTAT CheckMoveFeasibility(float sx, float sy, float dx, float dy, float *cx, float *cy){
	RETSTAT		retSt;
	float		tcx, tcy, ycd;

	*cx = 0.0;
	*cy = 0.0;
	tcx = 0.0;
	tcy = 0.0;
	ycd = 0.0;

	retSt = GetConstraintPoint(sx, sy, dx, dy, &tcx, &tcy);
	if(retSt != RT_NORMAL)
		return(PH_NORMAL);

	if((tcx == 0.0) && (tcy == 0.0)){
		return(PH_NORMAL);
	}

	ycd = sy + (((dy - sy)/(dx - sx)) * (tcx - sx));

	if(ycd > tcy){
		*cx = tcx;
		*cy = tcy;
	}
	
	return(PH_NORMAL);
}

PHASTAT CheckMoveFeasibilityEx(float sx, float sy, float dx, float dy, float tvAngle, float *cx, float *cy){
	RETSTAT		retSt;
	float		tcx, tcy, ycd;

	*cx = 0.0;
	*cy = 0.0;
	tcx = 0.0;
	tcy = 0.0;
	ycd = 0.0;

	retSt = GetConstraintPointEx(sx, sy, dx, dy, tvAngle, &tcx, &tcy);
	if(retSt != RT_NORMAL)
		return(PH_NORMAL);

	if((tcx == 0.0) && (tcy == 0.0)){
		return(PH_NORMAL);
	}

	ycd = sy + (((dy - sy)/(dx - sx)) * (tcx - sx));

	if(ycd > tcy){
		*cx = tcx;
		*cy = tcy;
	}
	
	return(PH_NORMAL);
}
